#!/usr/bin/env python
# -*- coding: UTF-8 -*-
"""
@Project ：刘骏轩2025讯飞 
@File ：guolidarLineControl.py
@Author ：ljx
@Date ：2025/8/9 上午4:39 
"""
#!/usr/bin/env python
# -*- coding: UTF-8 -*-
"""
@Project ：刘骏轩2025讯飞 
@File ：lidarLineControl.py
@Author ：ljx
@Date ：2025/8/8 上午12:56 
"""

import cv2
import time
import rospy
import math
import threading
import numpy as np
import numpy as np
from guoLineFind import LineFinder
from GetCvPicture import PicGetter
from geometry_msgs.msg import Twist
from std_srvs.srv import Trigger, TriggerResponse
from sensor_msgs.msg import LaserScan


class PD_Controler:
    def __init__(self, KP, KD, Maxoutput):
        self.KP = KP
        self.KD = KD
        self.last_error = 0
        self.last_last_error = 0
        self.max_output = Maxoutput
        self.target = 0
        self.output = 0

    def Revise(self, now):
        """
        PD 控制角速度
        :param now:当前值，now = 中点坐标的平均值减去320  now > 0 :则说明中线在小车的靠右位置 小车需右转 , now < 0 : 中线在小车的靠左位置 , 小车需左转
        :return: 控制输出
        """
        # 采用增量式PD
        # 角速度为负时,是顺时针，即向右偏 ; 为正时,是逆时针，即向左偏
        error = self.target - now
        self.output += self.KP * (error - self.last_error) + self.KD * (
                error + self.last_last_error - 2 * self.last_error)

        self.output = np.clip(self.output, -self.max_output, self.max_output)
        self.last_last_error = self.last_error
        self.last_error = error

        return self.output

class PID_Controller:
    def __init__(self, kp, kd, ki, maxoutput):
        self.KP = kp
        self.KI = ki
        self.KD = kd
        self.last_error = 0
        self.last_last_error = 0
        self.max_output = maxoutput
        self.target = 0
        self.output = 0

    def Revise(self, now):
        """
        PD 控制角速度
        :param now:当前值，now = 中点坐标的平均值减去320  now > 0 :则说明中线在小车的靠右位置 小车需右转 , now < 0 : 中线在小车的靠左位置 , 小车需左转
        :return: 控制输出
        """
        # 采用增量式PD
        # 角速度为负时,是顺时针，即向右偏 ; 为正时,是逆时针，即向左偏
        error = self.target - now
        self.output += ((self.KP * (error - self.last_error) +
                        self.KD * (error + self.last_last_error - 2 * self.last_error)) +
                        self.KI * error)

        self.output = np.clip(self.output, -self.max_output, self.max_output)
        self.last_last_error = self.last_error
        self.last_error = error

        return self.output

class Line_Controler:
    def __init__(self):
        self.cap = PicGetter()
        self.PD_control = PD_Controler(KP=0.85,
                                       KD=0.45,
                                       Maxoutput=2.0)
        self.finder = LineFinder()
        # 表示是否在巡线
        self.active = False
        self.cmd_pub = None
        self.control_thread = None
        # 发布速度消息
        self.cmd_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)

        self.emergency = False

        self.lidar = lidar()

    def avoid_Ob(self):
        """
        写死避障
        :return:
        """
        twist = Twist()
        rate = rospy.Rate(20)
        start = time.time()
        while time.time() - start <= 2.1:
            twist.linear.y = 0.2
            twist.linear.x = 0
            self.cmd_pub.publish(twist)
            rate.sleep()

        start = time.time()
        while time.time() - start <= 3.1:
            twist.linear.y = 0
            twist.linear.x = 0.2
            self.cmd_pub.publish(twist)
            rate.sleep()

        start = time.time()
        while time.time() - start <= 2.15:
            twist.linear.y = -0.2
            twist.linear.x = 0
            self.cmd_pub.publish(twist)
            rate.sleep()

        rospy.loginfo("avoid is over")

    def start(self):
        """
        这个服务只会启动一次，所以没有避免考虑过多，比如说self.image_sub 和 self.cmd_pub 重复申请的问题
        :return:
        """
        if self.active:
            rospy.logwarn("Controller is already running")
            return
        # 从参数服务器获取外线参数
        self.finder.outLine = rospy.get_param("outLine", "right")

        rospy.loginfo(f"外线：{ self.finder.outLine}")
        
        self.active = True
        # self.control_thread = threading.Thread(target=self.run)
        # self.control_thread.daemon = True  # 设置为守护线程
        # self.control_thread.start()
        self.lidar.start()
        # 开始订阅图片
        self.cap.start()
        rospy.loginfo("Control thread started")
        self.run()

    def lidar_Revise(self):
        twist = Twist()
        rate = rospy.Rate(10)

        threshold = 0.03

        while True:
            with self.lidar.lock:
                msg = self.lidar.lidarMes

            if msg is None:
                rospy.sleep(0.01)
                continue

            n = int(15 * math.pi / 180 / msg.angle_increment)

            mid = len(msg.ranges) // 2

            # 记录总差值
            sum_error = 0

            for i in range(1, n):
                if msg.range_min <= msg.ranges[mid + i] <= msg.range_max and msg.range_min <= msg.ranges[mid - i] <= msg.range_max:
                    # right - left
                    sum_error += msg.ranges[mid + i] - msg.ranges[mid - i]

            rospy.loginfo(f"当前总误差：{sum_error}")

            # 小于某一个设定的阈值，我们就认为它已经成功矫正
            if abs(sum_error) <= threshold:
                rospy.loginfo("雷达矫正已完成")
                break

            # 进行PD修正
            output = self.lidar.controller.Revise(sum_error)
            rospy.loginfo(f"PD角速度输出:{output}")
            twist.angular.z = output
            self.cmd_pub.publish(twist)

            rate.sleep()

    def run(self):
        # rate = rospy.Rate(20)  # 25Hz

        while not rospy.is_shutdown() and self.active:

            img = self.cap.Get()
            if img is None:
                continue
            try:
                if self.lidar.avoid_Flag:
                    # 1、首先进行雷达矫正
                    self.lidar_Revise()
                    # 2、雷达写死避障
                    self.avoid_Ob()
                    # 3、结束雷达检测子线程
                    self.lidar.threading_Stop()

                    self.lidar.avoid_Flag = False
                else:
                    msg = Twist()
                    error = self.finder.solve(img, 'test')
                    print("area:", self.finder.area)
                    if self.finder.area == "transition":
                        if self.finder.transition_Counts == 0:
                            msg.linear.x = 0.15
                            if self.finder.outLine == "right":
                                msg.angular.z = 0.35
                            else:
                                msg.angular.z = -0.38
                        elif self.finder.transition_Counts == 1:
                            if self.finder.outLine == "right":
                                msg.linear.x = 0.05
                                msg.angular.z = 0.68
                            else:
                                msg.angular.z = -0.2
                                start = time.time()
                                rate = rospy.Rate(10)
                                while time.time() - start < 0.05:
                                    self.cmd_pub.publish(msg)
                                    rate.sleep()
                                msg.linear.x = 0.08
                                msg.angular.z = -0.72

                            start = time.time()
                            rate = rospy.Rate(20)
                            while time.time() - start < 0.1:
                                self.cmd_pub.publish(msg)
                                rate.sleep()
                    elif self.finder.area == "transition2":
                        msg.linear.x = 0.35
                        if self.finder.outLine == "right":
                            msg.angular.z = 0.92
                        else:
                            msg.angular.z = -0.92
                        start = time.time()
                        rate = rospy.Rate(20)
                        while time.time() - start < 0.18:
                            self.cmd_pub.publish(msg)
                            rate.sleep()
                    elif self.finder.area == "end":
                        msg.linear.x = 0.1
                        msg.linear.y = -0.07
                        if not self.emergency:
                            self.emergency = True
                            rospy.loginfo("Attention!!!")
                    elif self.finder.area == "stop":
                        rospy.loginfo("Line Finish")
                        break
                    else:
                        if self.emergency:
                            break
                        msg.linear.x = 0.25
                        if error is not None:
                            output = self.PD_control.Revise(error) if abs(error) > 0.05 else 0
                            if output is None:
                                continue
                            rospy.loginfo(f"output:{output}")
                            msg.angular.z = output

                    self.cmd_pub.publish(msg)
            except Exception as e:
                rospy.logerr(f"控制出错: {e}")
                rospy.sleep(0.1)  # 出错时稍作暂停

            # rate.sleep()

        # 停止机器人
        stop_msg = Twist()
        self.cmd_pub.publish(stop_msg)

        self.active = False

        rospy.loginfo("Control thread stopped")


class lidar:
    def __init__(self):
        # 使用PD用于停泊时的位姿矫正
        self.controller = PID_Controller(kp=0.6, kd=0.8, ki=0.3, maxoutput=0.2)

        self.lidarMesSub = None
        self.lidarMes = None

        # 是否需要避障
        self.avoid_Flag = False

        # 建立一个线程不断读取雷达消息，并判断当前是否需要进行一个雷达避障的程序
        self.detectAvoidThreading = None
        self.lock = threading.Lock()  # 添加线程锁
        self.ThreadingRunFlag = False  # 是否正在执行雷达检测

    def threading_work(self):
        while self.lidarMes is None and not rospy.is_shutdown():
            rospy.logwarn("等待雷达数据中...")
            rospy.sleep(0.05)

        while self.ThreadingRunFlag and not rospy.is_shutdown():
            # 下面会写具体判断前方是否有障碍物的代码
            with self.lock:
                msg = self.lidarMes

            if msg is None:
                rospy.sleep(0.01)
                continue

            n = int(15 * math.pi / 180 / msg.angle_increment)
            ranges = msg.ranges
            mid = len(ranges) // 2

            # 避免越界
            start_idx = max(mid - n, 0)
            end_idx = min(mid + n + 1, len(ranges))

            distance = []

            for i in range(start_idx, end_idx):
                if msg.range_min <= ranges[i] <= msg.range_max:
                    distance.append(ranges[i])

            if not distance:
                rospy.sleep(0.05)
                continue

            avg_distance = np.mean(distance)
            rospy.loginfo(f"前方距离为：{avg_distance}")

            if  avg_distance <= 0.55:
                rospy.logwarn("前方检测到障碍物，触发避障标志！")
                self.avoid_Flag = True
                break

    def threading_Start(self):
        rospy.loginfo("启动雷达判断子线程，不断检测周环境")
        self.avoid_Flag = False
        self.ThreadingRunFlag = True

        self.detectAvoidThreading = threading.Thread(target=self.threading_work)
        self.detectAvoidThreading.daemon = True  # 设置为守护线程
        self.detectAvoidThreading.start()

    def threading_Stop(self):
        if self.ThreadingRunFlag:
            rospy.loginfo("正在关闭雷达检测子线程...")
            self.ThreadingRunFlag = False
            self.detectAvoidThreading.join()
            rospy.loginfo("雷达检测子线程关闭完成")

    def start(self):
        self.lidarMesSub = rospy.Subscriber('/scan', LaserScan, self.lidar_callback)
        rospy.loginfo("启动检测障碍物")
        self.threading_Start()

    def lidar_callback(self, msg):
        with self.lock:
            self.lidarMes = msg


class LineService:
    def __init__(self):
        self.control = Line_Controler()
        self.service = None

    def doRequest(self, req):
        if self.control.active:
            return TriggerResponse(False, "Already running")

        # 运行控制逻辑
        self.control.start()

        return TriggerResponse(True, "Line control started")

    def start(self):
        rospy.loginfo("Line Service is ready")
        self.service = rospy.Service("LinePatrolService", Trigger, self.doRequest)


if __name__ == '__main__':
    rospy.init_node('LinePatrol', anonymous=True)

    service = LineService()

    service.start()

    rospy.spin()
